#define BLOB_MIN_SIZE 500        //Minimum blob size in square pixels
#define BLOB_MAX_SIZE 500        //Minimum blob size in square pixels

#define CENTER 176               //Center x pixel of the camera
#define ERROR_RANGE 70           //Range to the left and right of the center pixel


//Claw Functions
int openclaw(){
	set_servo_position(2,1400);}
int closeclaw(){
	set_servo_position(2,700);}
int raiseclaw(){
	mrp(0,275,250L);}	
int lowerclaw(){
	mrp(0,350,-250L);}
	

int crossthebufferandgettothewall(){
	/*
	create_drive_straight(200);
	while(analog10(7)>300){   //Go until we see the cutoff
			sleep(.02);}
	create_stop(); */
	create_drive_straight(400);   // go 15 cm further
	sleep(2.45);
	create_stop();
	
	create_spin_CW(200); //turn to face the wall
	sleep(1.0);
	create_stop();
	
	while(!gc_rbump || !gc_lbump){  //and go until you hit the wall
		create_drive_straight(200);
		create_sensor_update();
		}
	create_stop();
}


int sweep(){
	set_servo_position(1,0);
	sleep(2.0);
	create_drive(-120, 275);      //sweep back in a circle
	sleep(4.0);
	set_servo_position(1,1300);   // put the arm back
	create_stop();
	set_servo_position(1,1900);

}


int grabthepoms(){
	
	sleep(50.0);  //#########################################################################
	create_drive(120,275);  // reverse sweep circle back to the wall
	sleep(4.0);	
	create_stop();	
	sleep(.5);
	create_spin_CCW(200);  // face the poms
	sleep(1.0);
	create_stop();
	create_drive_straight(100);
	sleep(1.0);
	create_stop();
	
	lowerclaw();
	sleep(1.0);
	set_servo_position(2,1400);
	sleep(2.0);
	
/*	while(!digital(8)){                   //go until we hit the wall
		create_drive_straight(-100);
		create_sensor_update();
		sleep(0.2);
		}*/
	create_drive_straight(-100);
	sleep(2.0);
	openclaw();
	sleep(3.0);
	create_stop();
	
	openclaw();
	closeclaw();          //grab the poms
	sleep(1.0);
	create_drive_straight(200);    //go backward 
	sleep(3.0);
		
}
/*
int grabbotguy(){
	int botguy = 1;
	create_drive(120,275);  // reverse sweep circle back to the wall
	sleep(2.0);	
	create_stop();	
	sleep(.5);
	
	lowerclaw();
	sleep(1.0);
	openclaw();
	sleep(2.0);
	
	 while(!botguy){
            track_update();
            if(track_size(0,0) > BLOB_MAX_SIZE){                     //if there is a big enough blob
                if(track_x(0,0) < (CENTER + ERROR_RANGE) && track_x(0,0) > (CENTER - ERROR_RANGE)){
                create_stop();
				closeclaw();
				botguy = 1;
				}
			}
                
                if(track_x(0,0) < (CENTER - ERROR_RANGE +20)){           //if blob is on the left,
                    create_spin_CW(75);
					sleep(.4);
					create_stop();
                }
                else if(track_x(0,0) > (CENTER + ERROR_RANGE +20)){      //if blob is on the right,
                    create_spin_CCW(75);
					sleep(.4);
					create_stop();

                }
                else if(track_x(0,0) < (CENTER + ERROR_RANGE) && track_x(0,0) > (CENTER - ERROR_RANGE)){                                           //if blob is within ERROR_RANGE,
                    create_drive_straight(75);
				    sleep(.4);
					create_stop();
                }
                
            
            else{                                                   //if no blob found,
                create_spin_CCW(75);
				sleep(.4);
				create_stop();
                }
         
	
	closeclaw();          //grab the poms
	sleep(1.0);
	create_drive_straight(200);    //go backward 
	sleep(3.0);
		
	}
}
*/
int returntobase(){
	
	create_spin_CW(200);       // reverse face the back wall
	sleep(1.0);
	create_stop();
	
	create_drive_straight(-200);  // go backwards
	sleep(4.0);
	
		/*while(!digital(8)){                   //go until we hit the wall
		create_drive_straight(-100);
		create_sensor_update();
			sleep(.05);
		}*/
		create_stop();
		create_drive_straight(50);  // back up a smidgen
		sleep(0.5);
		create_stop();

	
	create_spin_CCW(200);  // reverse face the wetlands
	sleep(1.0);
	create_stop();
	
	/*create_drive(-200, 400);
	sleep(1.5);
	create_drive(-200, -400);
	sleep(1.5);*/
	create_drive_straight(-200);  // drive into the wetlands
	sleep(4.5);
	create_stop();

	openclaw();	       //drop the poms
	sleep(1.0);
	raiseclaw();
	sleep(1.0);
	//closeclaw();	
	freeze(0);

}


int create(){
	crossthebufferandgettothewall();
	sweep();
	grabthepoms();
	returntobase();
	/*
	crossthebufferandgettothewall();
	grabthepoms();
	returntobase();
	
	crossthebufferandgettothewall();
	grabthepoms();
	returntobase();*/
}


int main()
{
create_connect();
create_safe();
create_sensor_update();
	
freeze(0);
	wait_for_light(0);              //#################################### TKAE THIS OUT FOR COMPETITON #####################################
	run_for(115.0, create);
ao();
create_stop();        
create_disconnect();
}
